﻿/*                       NOTICE
 * 
 * This software was produced for the U. S. Government
 * under Contract No. FA8721-10-C-0001, and is
 * subject to the Rights in Noncommercial Computer Software
 * and Noncommercial Computer Software Documentation Clause
 * (DFARS) 252.227-7014 (JUN 1995)
 * 
 * Approved for Public Release: 11-2435
 * © 2011 The MITRE Corporation. All Rights Reserved. 
 * 
 * Licensed under the Apache License, Version 2.0 (the "License");
 * you may not use this file except in compliance with the License.
 * You may obtain a copy of the License at
 * 
 * http://www.apache.org/licenses/LICENSE-2.0
 * 
 * Unless required by applicable law or agreed to in writing, software
 * distributed under the License is distributed on an "AS IS" BASIS,
 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
 * See the License for the specific language governing permissions and
 * limitations under the License.
 * 
 * Finger tracking comes courtesy of Stefan Stegmueller's Candescent NUI project.
 * Source and more details may be found at
 * 
 * http://candescentnui.codeplex.com/
 * 
 * © 2011 Stefan Stegmueller
 * All rights reserved.
 */

using System;
using System.Collections.Generic;
using System.Linq;
using System.Text;
using System.Threading;
using CCT.NUI.Core;
using CCT.NUI.HandTracking;
using CCT.NUI.Core.Clustering;
using CCT.NUI.Core.DataSources;
using TUIO;

namespace GRIP
{
    /// <summary>
    /// FingerFinder is designed to plug GRIP into CandescentNUI's finger detection framework. It's designed to work with
    /// the raw depth data - but since we use the SDK's skeleton tracking, we can dynamically feed it hand information based
    /// on where the skeleton tracker assumes the hands are. This actually works quite well, even at an angle or at a distance.
    /// </summary>
    class FingerFinder : IClusterDataSource
    {
        /// How far in front of your body a hand has to be in order to enable finger tracking.
        /// Set this reasonably far so that finger tracking isn't engaged when your hands are by
        /// your sides, for example.
        public int HandBodyDifferenceThreshold;

        /// Right now we just put a box around your hand and take all of the pixels within it.
        /// These map to X, Y, and Z lengths of the box, respectively.
        public int HandWidth;
        public int HandHeight;
        public int HandLength;

        // Used to distinguish which fingers are which based on angle differences
        public int ThumbIndexThreshold;
        public int ThumbMiddleThreshold;
        public int ThumbPinkyThreshold;
        public int IndexPinkyThreshold;
        public int MiddlePinkyThreshold;

        // Determines how long to buffer entering and exiting fingers
        public int EntryBufferTime;
        public int ExitBufferTime;

        // Any movement below this distance will be ignored
        public int JitterFilterRadius;

        // Any movement above this distance will be clipped to this distance
        public int SmoothingRadius;

        // The multiplier used for calculating hand angles (negative for left hands, positive for right)
        private double AngleScalingFactor;

        // Pre-computed reciprocals for micro-optimization action
        private static float ScaleReciprocalX = 1.0f / 640.0f;
        private static float ScaleReciprocalY = 1.0f / 480.0f;

        // A huge pool of points for generating hand clusters.
        // Keeping them around saves a ton of garbage collection time.
        private Point[] pointPool;
        private Point zeroPoint = new Point(0, 0, 0);

        // Point for holding the wrist vector (when doing finger angle calculations)
        private Point wristVector = new Point(0, 0, 0);

        // ID to Contact map (0 = thumb, 1 = index finger...)
        private TuioContact[] contactLookup = new TuioContact[5];

        // Used to sort points based on their angle
        private static IComparer<AngularFingerPoint> fingerComparer = new FingerComparer();

        // Keeps track of last known coordinates for smoothing
        private Point[] lastKnownFingerPositions = new Point[5];

        // A bunch of stuff carried over from the Kinect
        private TuioContact Wrist, Hand, Thumb, IndexFinger, MiddleFinger, RingFinger, Pinky, Body;
        private KinectAdapter kinect;
        private byte[] depthData;
        
        // Flag for killing new thread work orders if one is still going
        private bool working = false;

        // Entry and exit time buffer for points
        private BufferFilter<int> filter;
        
        // Reset Event for threading, so we know when this thread is done
        private AutoResetEvent resetEvent;

        public enum HandType
        {
            Left,
            Right
        }

        public FingerFinder(AutoResetEvent resetEvent, KinectAdapter kinect, HandType handType, FingerSettings settings)
        {
            this.kinect = kinect;
            if (handType == HandType.Left)
            {
                this.Thumb = kinect.ThumbLeft;
                this.IndexFinger = kinect.IndexFingerLeft;
                this.MiddleFinger = kinect.MiddleFingerLeft;
                this.RingFinger = kinect.RingFingerLeft;
                this.Pinky = kinect.PinkyLeft;
                this.Hand = kinect.HandLeft;
                this.Wrist = kinect.WristLeft;
                AngleScalingFactor = -180.0 / Math.PI;
            }
            else
            {
                this.Thumb = kinect.ThumbRight;
                this.IndexFinger = kinect.IndexFingerRight;
                this.MiddleFinger = kinect.MiddleFingerRight;
                this.RingFinger = kinect.RingFingerRight;
                this.Pinky = kinect.PinkyRight;
                this.Hand = kinect.HandRight;
                this.Wrist = kinect.WristRight;
                AngleScalingFactor = 180.0 / Math.PI;
            }
            this.Body = kinect.Spine;
            this.depthData = kinect.getDepthFrame();
            this.resetEvent = resetEvent;
            contactLookup = new TuioContact[] { this.Thumb, this.IndexFinger, this.MiddleFinger, this.RingFinger, this.Pinky };
            this.Size = new IntSize(640, 480);

            HandBodyDifferenceThreshold = settings.HandBodyDifferenceThreshold;
            HandWidth = settings.HandWidth;
            HandHeight = settings.HandHeight;
            HandLength = settings.HandLength;
            ThumbIndexThreshold = settings.ThumbIndexThreshold;
            ThumbMiddleThreshold = settings.ThumbMiddleThreshold;
            ThumbPinkyThreshold = settings.ThumbPinkyThreshold;
            IndexPinkyThreshold = settings.IndexPinkyThreshold;
            MiddlePinkyThreshold = settings.MiddlePinkyThreshold;
            EntryBufferTime = settings.EntryBufferTime;
            ExitBufferTime = settings.ExitBufferTime;
            JitterFilterRadius = settings.JitterFilterRadius;
            SmoothingRadius = settings.SmoothingRadius;
            filter = new BufferFilter<int>(EntryBufferTime, ExitBufferTime);

            pointPool = new Point[HandWidth * HandHeight * 4];
            for (int i = 0; i < pointPool.Length; i++)
                pointPool[i] = new Point(0, 0, 0);

            HandDataSourceSettings handSettings = new HandDataSourceSettings();
            HandDataSourceSettings.SetToAccurate(handSettings);
            handSettings.DetectCenterOfPalm = false;
            handSettings.MinimumDistanceBetweenFingerPoints = settings.MinimumDistanceBetweenFingerPoints;
            handSettings.MinimumDistanceIntersectionPoints = settings.MinimumDistanceIntersectionPoints;
            handSettings.MinimumDistanceFingerPointToIntersectionLine = settings.MinimumDistanceFingerPointToIntersectionLine;
            handSettings.MaximumDistanceBetweenIntersectionPoints = settings.MaximumDistanceBetweenIntersectionPoints;
            new HandDataSource(this, handSettings).NewDataAvailable += new CCT.NUI.Core.DataSources.NewDataHandler<HandCollection>(fingersReady);
        }

        /// <summary>
        /// Dynamically constructs clusters for the user's hands and passes them onto CandescentNUI for finger detection.
        /// Ideally this would BFS through the depth image, starting at the palm pixel the SDK gives you, but for now
        /// it just puts a box around that center pixel and grabs all the points within an acceptable depth range.
        /// </summary>
        public void processHand(object threadContext)
        {
            if (working)
                return;

            working = true;
            float bodyDepth = Body.zpos;
            float handDepth = Hand.zpos;

            if (bodyDepth - handDepth > HandBodyDifferenceThreshold)
            {
                ClusterData clusterData = new ClusterData();
                Cluster cluster = new Cluster(zeroPoint);
                int handPointCount = 0;
                int handX = (int)(Hand.xpos * 639);
                int handY = (int)(Hand.ypos * 479);
                int xMin = Math.Max(handX - HandWidth, 0);
                int yMin = Math.Max(handY - HandHeight, 0);
                int xMax = Math.Min(handX + HandWidth, 639);
                int yMax = Math.Min(handY + HandHeight, 479);

                for (int y = yMin; y < yMax; y++)
                    for (int x = xMin; x < xMax; x++)
                    {
                        int index = (y * 640 + (639 - x)) * 2;
                        int depth = depthData[index] | (depthData[index + 1] << 8);
                        if (depth > 0 && depth - handDepth < 60)
                        {
                            Point currentPoint = pointPool[handPointCount];
                            currentPoint.X = x;
                            currentPoint.Y = y;
                            currentPoint.Z = depth;
                            cluster.AddPoint(currentPoint);
                        }
                        handPointCount++;
                    }

                cluster.AllPoints = (IList<Point>)cluster.Points;
                cluster.CalculateVolume();
                clusterData.Clusters.Add(cluster);
                try
                {
                    NewDataAvailable(clusterData);
                }
                catch (Exception) // Rare bug with Candescent, just catch it and move on to the next frame
                {
                    working = false;
                    resetEvent.Set();
                }
            }
            else
            {
                Thumb.active = IndexFinger.active = MiddleFinger.active = RingFinger.active = Pinky.active = false;
                working = false;
                resetEvent.Set();
            }
        }

        private void fingersReady(HandCollection data)
        {
            Dictionary<int, AngularFingerPoint> incomingFingerLookup = processIncomingFingers(data);
            IDStateHolder<int> processedFingerIDs = filter.process(incomingFingerLookup.Keys);

            assignNewFingers(processedFingerIDs.NewIDs, incomingFingerLookup);
            updateExistingFingers(processedFingerIDs.ExistingIDs, incomingFingerLookup);
            unassignRemovedFingers(processedFingerIDs.RemovedIDs);

            working = false;
            resetEvent.Set();
        }

        private Dictionary<int, AngularFingerPoint> processIncomingFingers(HandCollection data)
        {
            Dictionary<int, AngularFingerPoint> incomingFingerLookup = new Dictionary<int, AngularFingerPoint>(5);
            if (data.HandsDetected)
            {
                int handX = (int)(Hand.xpos * 640);
                int handY = (int)(Hand.ypos * 480);
                wristVector.X = Wrist.xpos * 640 - handX;
                wristVector.Y = Wrist.ypos * 480 - handY;

                IList<FingerPoint> fingerPoints = data.Hands[0].FingerPoints;
                List<AngularFingerPoint> augmentedFingerPoints = new List<AngularFingerPoint>(fingerPoints.Count);

                // Figure out what angle this finger point is relative to the hand-wrist vector
                for (int i = 0; i < fingerPoints.Count; i++)
                {
                    FingerPoint finger = fingerPoints[i];
                    Point location = finger.Location;
                    AngularFingerPoint augmentedFinger = new AngularFingerPoint(location, finger.Id);
                    augmentedFinger.angle = calculateAngle(wristVector, new Point(location.X - handX, location.Y - handY, 0));
                    augmentedFingerPoints.Add(augmentedFinger);
                }
                // Sort based on left or right hand cardinality
                augmentedFingerPoints.Sort(fingerComparer);

                bool done = augmentedFingerPoints.Count < 3;

                // Assign the first finger as the index finger
                if (augmentedFingerPoints.Count == 1)
                {
                    augmentedFingerPoints[0].Id = 1;
                    incomingFingerLookup.Add(1, augmentedFingerPoints[0]);
                }
                // Check the angle between finger 0 and 1 to see if it should actually be a thumb
                else if (augmentedFingerPoints.Count > 1)
                {
                    AngularFingerPoint firstPoint = augmentedFingerPoints[0];
                    AngularFingerPoint secondPoint = augmentedFingerPoints[1];
                    double diff = secondPoint.angle - firstPoint.angle;
                    if (diff < ThumbIndexThreshold) // This is most likely an index and middle finger
                    {
                        firstPoint.Id = 1;
                        secondPoint.Id = 2;
                        incomingFingerLookup.Add(1, firstPoint);
                        incomingFingerLookup.Add(2, secondPoint);
                    }
                    else if(diff < ThumbMiddleThreshold) // This is most likely a thumb and index finger
                    {
                        firstPoint.Id = 0;
                        secondPoint.Id = 1;
                        incomingFingerLookup.Add(0, firstPoint);
                        incomingFingerLookup.Add(1, secondPoint);
                    }
                    else if (diff < ThumbPinkyThreshold) // This is most likely a thumb and middle finger
                    {
                        firstPoint.Id = 0;
                        secondPoint.Id = 2;
                        incomingFingerLookup.Add(0, firstPoint);
                        incomingFingerLookup.Add(2, secondPoint);
                    }
                    else // This is a thumb and a pinky
                    {
                        firstPoint.Id = 0;
                        secondPoint.Id = 4;
                        incomingFingerLookup.Add(0, firstPoint);
                        incomingFingerLookup.Add(4, secondPoint);
                        done = true;
                    }
                }

                // Take all the points up to the pinky and add them to the processing queue
                int j = 2;
                while (!done && j < augmentedFingerPoints.Count)
                {
                    AngularFingerPoint previousFingerPoint = augmentedFingerPoints[j - 1];
                    AngularFingerPoint currentFingerPoint = augmentedFingerPoints[j];
                    double diff = currentFingerPoint.angle - previousFingerPoint.angle;

                    if (previousFingerPoint.Id == 1 && diff < IndexPinkyThreshold) // This is an index and middle
                        currentFingerPoint.Id = 2;
                    else if (previousFingerPoint.Id == 1) // This is an index and pinky
                    {
                        currentFingerPoint.Id = 4;
                        done = true;
                    }
                    else if (previousFingerPoint.Id == 2 && diff < MiddlePinkyThreshold) // This is a middle and ring
                        currentFingerPoint.Id = 3;
                    else if (previousFingerPoint.Id == 2) // This is a middle and pinky
                    {
                        currentFingerPoint.Id = 4;
                        done = true;
                    }
                    else // This is a pinky
                    {
                        currentFingerPoint.Id = 4;
                        done = true;
                    }
                    incomingFingerLookup.Add(currentFingerPoint.Id, currentFingerPoint);
                    j++;
                }
            }

            return incomingFingerLookup;
        }

        private void assignNewFingers(List<int> newFingerPointIDs, Dictionary<int, AngularFingerPoint> newFingerPoints)
        {
            foreach (int Id in newFingerPointIDs)
            {
                updateFingerLocation(newFingerPoints[Id]);
                kinect.runContactAddedListeners(contactLookup[Id]);
            }
        }

        private void updateExistingFingers(List<int> updatedFingerPointIDs, Dictionary<int, AngularFingerPoint> updatedFingerPoints)
        {
            foreach (int Id in updatedFingerPointIDs)
            {
                AngularFingerPoint finger;
                if (updatedFingerPoints.TryGetValue(Id, out finger))
                {
                    Point oldLocation = lastKnownFingerPositions[Id];
                    float deltaX = finger.X - oldLocation.X;
                    float deltaY = finger.Y - oldLocation.Y;
                    float deltaZ = finger.Z - oldLocation.Z;
                    float dist = (float)Math.Sqrt(deltaX * deltaX + deltaY * deltaY + deltaZ * deltaZ);
                    // This move has to be big enough to not be considered noise
                    if (dist > JitterFilterRadius)
                    {
                        // If this move is bigger than the smoothing radius, trim it down
                        if (dist > SmoothingRadius)
                        {
                            float scalingFactor = SmoothingRadius / dist;
                            float newX = oldLocation.X + deltaX * scalingFactor;
                            float newY = oldLocation.Y + deltaY * scalingFactor;
                            float newZ = oldLocation.Z + deltaZ * scalingFactor;
                            updateFingerLocation(Id, newX, newY, newZ);
                        }
                        else
                            updateFingerLocation(finger);

                        kinect.runContactChangedListeners(contactLookup[Id]);
                    }
                }
            }
        }

        private void unassignRemovedFingers(List<int> removedFingerPointIDs)
        {
            foreach (int Id in removedFingerPointIDs)
            {
                contactLookup[Id].active = false;
                kinect.runContactRemovedListeners(contactLookup[Id]);
            }
        }

        private void updateFingerLocation(AngularFingerPoint finger)
        {
            int Id = finger.Id;
            TuioContact contact = contactLookup[Id];
            contact.active = true;
            contact.xpos = finger.X * ScaleReciprocalX;
            contact.ypos = finger.Y * ScaleReciprocalY;
            contact.zpos = finger.Z;
            lastKnownFingerPositions[Id] = finger.Location;
        }

        private void updateFingerLocation(int Id, float newX, float newY, float newZ)
        {
            TuioContact contact = contactLookup[Id];
            contact.active = true;
            contact.xpos = newX * ScaleReciprocalX;
            contact.ypos = newY * ScaleReciprocalY;
            contact.zpos = newZ;
            lastKnownFingerPositions[Id] = new Point(newX, newY, newZ);
        }

        private double calculateAngle(Point wristVector, Point fingerVector)
        {
            double angle = AngleScalingFactor * Math.Atan2(
                wristVector.X * fingerVector.Y - wristVector.Y * fingerVector.X,
                wristVector.X * fingerVector.X + wristVector.Y * fingerVector.Y);
            return angle < 0 ? angle + 360 : angle;
        }

        public bool IsRunning
        {
            get
            {
                return true;
            }
        }

        public void Start()
        {
        }

        public void Stop()
        {
        }

        public ClusterData CurrentValue
        {
            get;
            private set;
        }

        public IntSize Size
        {
            get;
            private set;
        }

        public event NewDataHandler<ClusterData> NewDataAvailable;

        private class AngularFingerPoint : FingerPoint
        {
            public double angle;

            public AngularFingerPoint(Point point, int Id)
                : base(point)
            {
                this.Id = Id;
            }
        }

        private class FingerComparer : IComparer<AngularFingerPoint>
        {
            public int Compare(AngularFingerPoint a, AngularFingerPoint b)
            {
                return a.angle.CompareTo(b.angle);
            }
        }
    }
}